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1 Introduction 


This paper presents a method of estimating all of the inertial parameters, the 
mass, the center of ma^ss, and the moments of inertia of each rigid body link of a 
robot manipulator using joint torque sensing. Determining these parameters from 
measurements or computer models is generally difficult and involves some approxi¬ 
mations to handle the complex shapes of the arm components. Typically, even the 
manufacturers of manipulators do not know accurate values of these parameters. 

The degree of uncertainty in inertial parameters is an important factor in judg¬ 
ing the robustness of model-based control strategies. A common objection to the 
computed torque methods, which involve full dynamics computation (e.g., Luh, 
Walker, and Paul, 1980), is their sensitivity to modelling errors, and a variety of 
alternative robust controllers have been suggested (Samson, 1983; Slotine, 1984; 
Spong, Thorp, and Kleinwaks; 1984, Gilbert and Ha, 1984). Typically these robust 
controllers express modelling errors as a differential inertia matrix and coriolis and 
gravity vectors, but in so doing, no rational basis is provided for the source of er¬ 
rors or the bounds on errors. The error matrices and vectors combine kinematic 
and dynamic parameter errors, but kinematic calibration is sufficiently developed 
so that very little error can be expected in the kinematic parameters (Whitney, 
Lozinski, and Rourke , 1984). 

One aim of this work is to place similar bounds on inertial parameter errors by 
explicitly identifying the inertial parameters of each link that go into the making of 
the inertia matrix and coriolis and gravity vectors. Our work in load identification 
(Atkeson, An, and Hollerbach, 1985) suggests, for example, that mass can be accu¬ 
rately identified to within 1 %. Therefore, an assumption of 50% error in link mass 
in verifying a robust control formulation (Spong, Thorp, and Kleinwaks, 1984) is an 
unreasonable basis for argument. Slotine (1984) suggests that errors of only a few 
percent in inertial parameters make his robust controller superior to the computed 
torque method, but it may well be that these parameters can be identified more 
accurately than his assumptions. 

As an alternative approach we propose estimating the inertial parameters on the 
basis of direct dynamic measurements. The same algorithms used to identify load 
inertial parameters (Atkeson, An, and Hollerbach, 1985) can be modified to find 
link inertial parameters of a robot arm made up of rigid parts. The Newton-Euler 
dynamic equations are used to express the measured forces and torques at each 
joint in terms of the product of the measured movements of the rigid body links 
and the unknown link inertial parameters. These equations are linear in the inertial 
parameters. However, unlike load estimation, the only sensing is one component 
of joint torque, inferred from motor current. Coupled with restricted movement 
near the base, it is, therefore, not possible to find all the inertial parameters of the 
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proximal links. As will be seen, these missing parameters have no effect on the 
control of the arm. 

In this paper, manipulators with only revolute joints are discussed since handling 
prismatic joints requires only trivial modifications to the algorithm. The proposed 
algorithm was verified by implementation on the MIT Serial Link Direct Drive Arm. 

I. 1 Previous Work 

Mayeda, Osuka, and Kangawa (1984) required three sets of special test motions to 
estimate the coefficients of a closed-form Lagrangian dynamics formulation. The 10 
inertial parameters of each link are lumped into these numerous coefficients, which 
are redundant and susceptible to numerical problems in estimation. On the other 
hand, every coefficient is identifiable since these coefficients represent the actual 
degrees of freedom of the robot. By sensing torque from only one joint at a time, 
their algorithm is more susceptible to noise from transmission of dynamic effects of 
distant links to the proximal measuring joints. For efficient dynamics computation, 
the recursive dynamics algorithms require the link parameters explicitly. Though 
recoverable from the Lagrangian coefficients, it is better to estimate the inertial 
parameters directly. Though this algorithm was implemented on a PUMA robot, 
it is difficult to interpret the results because of dominance of the dynamics by the 
rotor inertia and friction. 

Mukerjee (1985) directly applied his load identification method to link identi¬ 
fication, by requiring full force-torque sensing at each joint. Instrumenting each 
robot link with full force-torque sensing seems impractical, and is actually unnec¬ 
essary given joint torque sensing about the rotation axis. Partially as a result, he 
does not address the issue of unidentifiability of some inertial parameters. Also, he 
did not verify his algorithm by simulation or by implementation. 

Olsen and Bekey (1985) presented a link identification procedure using joint 
torque sensing and special test motions with single joints. The unidentifiability 
of certain inertial parameters was not resolved, and the least squares estimation 
procedure written as a generalized inverse would fail because of linear dependence of 
some of the inertial parameters. Again, their procedure was not tested by simulation 
or by actual implementation on a robot arm. 

Neuman and Khosia (1985) developed a hybrid estimation procedure combining 
a Newton-Euler and a Lagrange-Euler formulation of dynamics. Simulation results 
for a three degree-of-freedom cylindrical robot were presented, and the unidentifi¬ 
ability of certain inertial components was addressed. For some reason they state 
link mass must be known for a linear estimation procedure, but such a restriction 
does not exist with our method. Though planning to work with the CMU DDArm 

II, they have not yet presented experimental results. 
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Figure 1: Coordinate origins and location vectors for link identification. 

2 ESTIMATION PROCEDURE 

2.1 Formulation of Newton-Euler Equations 

In our work in load identification (Atkeson, An, and Hollerbach, 1985), the Newton- 
Euler equations for a rigid body load were formulated to be linear in the unknown 
inertial parameters. Then simple linear least squares method was used to estimate 
those parameters. By treating each link of a manipulator as a load, this formulation 
can be extended to the link estimation problem. The differences in the equations 
are that only one component of force or torque is sensed and that the forces and 
torques from distal links are summed and transmitted to the proximal joints. 

Consider a manipulator with n joints (Figure 1). Each link i has its own local 
coordinate system P,- fixed in the link with its origin at joint The joint force and 
torque due to the movement of its own link can be expressed by simply treating the 
link as a load and applying the previously developed equations for load identification 
(Atkeson, An, and Hollerbach, 1985): 
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or more compactly, 

Wii = Ai<j>i ( 1 ) 

where Wij is the wrench (vector of forces and torques) at joint i due to movement of 
link j alone. A,- is the kinematic matrix that describes the motion of link i and is 
the vector of unknown link inertial parameters. All of the quantities are expressed 
in the local joint i coordinate system. The formulation of the above Newton-Euler 
equations were already presented in the load identification paper (Atkeson, An, and 
Hollerbach), and are summarized in the Appendix of this paper for a reference. 

The total wrench w,- at joint i is the sum of the wrenches for all links j 
distal to joint 

N 

W( = Yl Wii (2) 

i=* 

Each wrench w,j at joint i is determined by transmitting the distal wrench Wjj 
across intermediate joints. This is a function of the geometry of the linkage only. 
The forces and torques at neighboring joints are related by 


_ _ [®tX] • Rt _ _ J^i+i,t+i 

or more compactly 



= Ti ( 4 ) 

where 

R,= the rotation matrix rotating the link ^ + 1 coordinate system to the link i 
coordinate system, 

s,= a vector from the origin of the link i coordinate system to the link z-f 1 co¬ 
ordinate system, and 
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T,-=: a wrench transmission matrix. 

To obtain the forces and torques at the joint due to the movements of the 
link, these matrices can be cascaded: 


Wij —• • • 'TjWjj 

(^) 

where • • • Tj A, and U,j = Aj. A simple matrix expression for a serial 

kinematic chain (in this case a six joint arm) can be derived from (2) and (5): 
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This equation is linear in the unknown parameters, but the left side is composed 
of a full force-torque vector at each joint. Since only the torque about the joint 
axis can usually be measured, each joint wrench must be projected onto the joint 
rotation axis (typically [0,0,1] in internal coordinates), reducing (6) to 


t=KiP (7) 

where r,- = [0,0,0,0,0, l]-w, is the joint torque of the link, V'= [^i» 02 5 ^35 5 ^55 ^6 

and Kij = [0,0,0, 0,0, l] • U,j when the corresponding entry in (6) is nonzero. For 
an n-link manipulator, r is a n x 1 vector, tpis a. lOn x 1 vector, and K is a n x lOn 
matrix. 


2.2 Estimating the Link Parameters 

Equation (7) represents the dynamics of the manipulator for one sample point. For 
extra data, (7) is augmented as: 



K(l) 


’■(1) 

K = 

. k{n) _ 

T = 



N = number of data points 


Unfortunately, one cannot apply simple least squares estimate: 


<l^e.tir.ate = (K^K)“'K^r 


( 8 ) 



because K^K is not invertible due to loss of rank from restricted degrees of freedom 
at the proximal links and the lack of full force-torque sensing. 

There are several ways to resolve this problem. One way to resolve this problem 
is to use the pseudo inverse to get a solution i/> to t = KV>. But since K is a 
potentially large nN x lOn matrix, the pseudo-inverse is computationally inefficient. 
Another simple method similar to the pseudo inverse is to use ridge regression 
(Marquardt and Snee, 1975). Ridge regression makes K^K invertible by adding a 
small number to the diagonal elements: 

V- = (K^K + dI,o„)-iK^r (9) 

The estimates are nearly optimal if d « A where A„„„ is the smallest 

non-zero eigenvalue of K^K. Other methods of solution, fundamentally different 
from the above two, are presented in the discussion section. 

3 Experimental Results 

Link estimation was implemented on the MIT Serial Link Direct Drive Arm (Figure 
2 ), a three link serial manipulator with no transmission mechanism between the 
motors and the links. The ideal rigid body dynamics is a good model for this arm, 
uncomplicated by joint friction or backlash typical of other manipulators. Hence the 
fidelity of this manipulator’s dynamic model suits estimation well. The coordinate 
system for this arm is defined in Figure 3. A set of inertial parameters is available 
for the arm (Table 1), determined by modeling with a CAD/CAM database (Lee, 
1983). These values can serve as a point of comparison for our method, but they 
may not be accurate because of modeling errors. 

The motors are rated at 660 Nm peak torque for joint 1 and 230 Nm for joints 
2 and 3 (Asada and Youcef-Toumi, 1984). Joint 1 is presently capable of an an¬ 
gular acceleration of 1150 deglsec^, joints 2 and 3 in excess of 6000 deg/sec^. In 
comparision, joint 1 of the PUMA 600 has a peak acceleration of 130 degjsec^ and 
joint 4 at the wrist 260 deg/sec^. Joint position is measured by a resolver and joint 
velocity by a tachometer. The tachometer output is of high quality and leads to 
good acceleration estimates after differentiation. The accuracy of the acceleration 
estimates plus high angular accelerations greatly improves inertia estimation. 

The joint torques are computed by measuring the currents of the 3 phase wind¬ 
ings of each motor (Asada, Youcef-Toumi, and Lim, 1984). For the three phase 
brushless permanent magnet motors of the direct drive arm, the output torque is 
related to the currents in the windings by: 

T = KT(Ia sin 6 lb sin(0 + 120) + Ic sin(0 + 240)) (10) 
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Figure 2: The MIT Serial Link Direct Drive Arm 
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Figure 3: The link coordinate system. 

The torque constant Kt for each motor is calibrated statically by measuring the 
force produced by the motor torque at the end of a known lever arm. The force 
is measured using a BarryWright Company Astek FS6-10A-200 6-axis force/torque 
sensor. Asada, Youcef-Toumi, and Lim have found that for a motor similar to 
the motors of our manipulator, the torque versus current relationship was non¬ 
linear, especially for small magnitudes of torques, and also varied as a function of 
the rotor position. However, for the results presented in this paper, the nonlinear 
effects were ignored since substantial portions of the movements in the experiments 
required large magnitudes of torques. Since the least squares algorithm minimizes 
the square of the error, torque errors for torques of small magnitudes do not affect 
the estimates very much. 

For the estimation results presented, 600 data points were sampled while the 
manipulator was executing 3 sets of fifth order polynomial trajectories in joint 
space. The specifications of the trajectories were: 

1. (330, 289.1, 230) to (80, 39.1, -10) degrees in 1.3s, 

2. (330, 269.1, -30) to (80, 19.1, 220) degrees in 1.3s, 

3. (80, 269.1, -30) to (330, 19.1, 220) degrees in 1.3s, 

Since K^K in (9) is singular, estimates for the 30 unknowns are computed by adding 
a small number (d = 10.0 << A„i,„(K^K) = 3395.0) to the diagonal elements of 
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Parameters 

Link 1 

Link 2 

Link 3 

m[Kg) 

67.13 

53.01 

19.67 

mCx(Kg • m) 

0.0 

0.0 

0.3108 

mCy 

2.432 

3.4081 

0.0 

mCg 

35.8257 

16.6505 

0.3268 

I„{Kg ■ m^) 

23.1568 

7.9088 

0.1825 

Ixy 

0.0 

0.0 

0.0 

^xz 

-0.3145 

0.0 

-0.0166 

^yy 

20.4472 

7.6766 

0.4560 

lyz 

-1.2948 

-1.5036 

0.0 

Izz 

0.7418 

0.6807 

0.3900 


Table 1: CAD-modeled inertial parameters. 


K^K. 

Typical results, obtained using ridge regression method, are shown in Table 2. 
Parameters that cannot be identified because of constrained motion near the base 
are denoted by 0.0*. The first nine parameters of the first link are not identifiable 
because this link has only one degree of freedom about its z axis. These nine 
parameters do not matter at all for the movement of the manipulator and thus can 
be arbitrarily set to 0.0. 

Other parameters marked by (f) can only be identified in linear combinations, 
indicated explicitly in Table 3. The ridge regression automatically resolves the 
linear combinations in a least squares sense. It can be seen that the estimated sums 
roughly match the corresponding sums inferred from the CAD-modeled parameters, 
but the sizeable discrepancy indicates that one parameter set may be more accurate 
than the other. 

To verify the accuracy of the estimated and the modeled parameters, the mea¬ 
sured joint torques are compared to the torques computed from the above two 
sets of parameters using the measured joint kinematic data. As shown in Figure 
4, the estimated torques match the measured torques very closely. The torques 
computed from the CAD/CAM modeled parameters do not match the measured 
torques as closely. This comparison verifies qualitatively that for control purposes 
the estimated parameters are in fact more accurate than the modeled parameters. 
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Parameters 

Link 1 

Link 2 

Link 3 

m{Kg) 

0 

• 

0 

0 

• 

0 

1.8920t 

mCx(Kg • m) 

0 

• 

0 

-0.1591 

0.4676 

mCy 

0 .0* 


0.0315 

mcz 

0 

• 

0 

0 .0* 

-1.0087t 

h,(Kg-m^) 

0 .0* 

4.1562t 

1.5276t 

Ixy 

0 

• 

0 

0.3894 

-0.0256 

Ixz 

0 

« 

0 

0.0118 

0.0143 

^VV 

0 .0* 

5.2129t 

1.8967t 

lyz 

0 

• 

0 

-0.6050t 

-0.0160 

^ZZ 

9.33598t 

-0.8194t 

0.3568 


Table 2: Estimated inertial parameters. 


Linear Combinations 

Estimated 

CAD-Modeled 

^S^zzh + Iyz2 

-1.0589 

-1.3565 

IXX 3 ~ lyyz 

-0.3691 

-0.2702 

IzZ2 ^XXs 

0.7082 

0.8632 

Izzi ^xx 2 ^xxz “H 

15.7029 

12.8169 

IxX2 ^XXs ^yV2 

0.4709 

0.4147 

“^S^zs m2Cy2 

-1.6863 

-3.0814 


Table 3: Parameters in linear combinations (I 2 = 0.45m.) 
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4 Discussion 


Good estimates of the link inertial parameters were obtained, as determined from 
the match of predicted torques to measured torques. The potential advantage 
of this movement-based estimation procedure for increased accuracy as well as 
convenience was demonstrated by the less accurately predicted torques based on 
the CAD-modeled inertial parameters. 

There are three groups of inertial parameters: fully identifiable, completely 
unidentifiable, and identifiable in linear combinations. Membership of a parameter 
in a group depends on the manipulator’s particular geometry. Some link inertial 
parameters are unidentifiable because of restricted motion near the base and the 
lack of full force-torque sensing at each joint. For the first link, rotation is only 
possible about its z axis. Suppose full force-torque sensing is available at joint 1. 
It can be seen from (1) that Ixxx^ ^xyn 3>iid lyy^ are unidentifiable because they have 
no effect on joint torque. Since the gravity vector is parallel to the z axis, is 
also unidentifiable. If it is now supposed that only torque about the z axis can be 
sensed, then all inertial parameters for link 1 become unidentifiable except . 

In a multi-link robot a new phenomenon arises. Some parameters can only be 
identified in linear combinations, because proximal joints must provide the torque 
sensing to identify fully the parameters of each link. Certain parameters from 
distal links are carried down to proximal links until a link appears with a rotation 
axis oriented appropriately for completing the identification. In between, these 
parameters appear in linear combinations with other parameters. This partial 
identifiability and the difficulty of analysis become worse as the number of links are 
increased. 

The ridge regression automatically resolves the linear combinations in a least 
squares sense, which make these inertial parameters appear superficially different 
from those derived by CAD modeling. An alternative method is generation and 
examination of the closed-form dynamics, which is a complex procedure for more 
than two degrees of freedom. 

A second alternative is a numerical analysis via singular value decomposition of 
K in (8), yielding (Golub and Van Loan, 1983) 

K = UEV^ 

where H = diag{cr,} and U and are orthogonal matrices. For each column 
of V there corresponds a singular value a,- which if not zero indicates that linear 
combination of parameters, vfV', is identifiable. Since K is a function only of the 
geometry of the arm and the commanded movement, it can be generated exactly 
by simulation rather than by actually moving the real arm and recording data with 
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inevitable noise. 

The above two rocedures isolate several sets of parameters whose linear combi¬ 
nations within each set are identifiable. Then we can arbirtrarily set all but one 
of the parameters of each set to zero and apply the estimation algorithm for the 
reduced set of fully identifiable parameters. As shown in Table 2 , for our 3 link 
manipulator, these two procedures result in grouping the 30 inertial parameters 
into the following categories: 

1 . fully identifiable, tn^Cy^^ /asya* ^ 2 ^* 2 > ^xy 2 ^ ^xz 2 

2 . completely unidentifiable: m 2 , mjc^, mi, mjc*,, mic^,, mic^,, 7 ^^,, 7 ^^^, 
■^yyn ^yzi 

3. identifiable in linear combinations: m 3 , 7 ^* 3 , lyy^, mjCy,, lyy^, 

^ZZ2i ^ZZi 

The completely unidentifiable parameters can be arbitrary set to zero. For the 
linear combination parameters, the combinations of these parameters that can be 
identified together are shown in table 3. To obtain a particular solution for these 
parameters, one can set ^ind 7**2 to zero. Then, the remaining 6 

parameters of this category can be treated as fully identifiable parameters along 
with the 9 parameters in the first category. Those colunms of K which correspond 
to the 15 zeroed parameters are taken out, reducing K^K to a 15 x 15 full rank 
matrix. Now, simple least squares can be applied to estimate the 15 identifiable 
parameters. For our implementation experiments, the results of this method agreed 
with the results of ridge regression presented in the previous section. 

Although not as simple as the ridge regression method, these methods are attrac¬ 
tive since it allows us to reformulate the dynamics in terms of only the identifiable 
parameters. This then can increase the efficiency of the corresponding inverse dy¬ 
namics computation for controller implementations (Hollerbach and Sahar, 1983). 
We are still investigating these issues with eventual goals of studying the effec¬ 
tiveness of different control algorithms using the estimated dynamic model and 
analyzing their robustness with modelling errors. 


References 


Asada, H., and Youcef-Toumi, K. 1984 Analysis and design of a direct-drive arm 
with a five-bar-link parallel drive mechanism. ASME J. Dynamic Systems, Meas., 
Control. 106: 225-230. 


13 



Asada, H., Youcef-Toumi, K., and Lim, S.K. 1984 (Dec. 12-14). Joint torque 
measurement of a direct-drive arm. Proc. 2Srd Conf. Decision and Control. Las 
Vegas, pp. 1332-1337. 

Atkeson, C.G., An, C.H., and Hollerbach, J.M. 1985 (Dec. 11-13). Rigid body 
load identification for manipulators. Proc. 24 th Conf. Decision and Control. Fort 
Lauderdale, Florida. 

Gilbert, E.G., and Ha, I.J. 1984 An approach to nonlinear feedback control with 
applications to robotics. IEEE Trans. Systems, Man, Cybern.. SMC-14: 879-884. 

Golub, G.H., and Van Loan, C.F. 1983 Matrix computations. John Hopkins 
University Press 

Hollerbach, J. M., and Sahar, G. 1983 Wrist-partitioned inverse kinematic 
accelerations and manipulator dynamics. Int. J. Robotics Research. 2:(4): 61-76. 

Lee, K. 1983 (December). Shape Optimization of Assemblies Using Geometric 
Properties. Ph.D. thesis, MIT, Mechanical Engineering Department 

Luh, J.Y.S., Walker, M.W., and Paul, R.P.C. 1980 Resolved-acceleration control 
of mechanical manipulators. IEEE Trans. Auto. Contr.. AC-25:(3): 468-474. 

Marquardt, D.W., and Snee, R.D. 1975 Ridge regression in practice. Amer. 
Statistician. 29: 3-20. 

Mayeda, H., Osuka, K., and Kangawa, A. 1984 (July 2-6). A new identification 
method for serial manipulator arms. Preprints IF AC 9th World Congress. 
Budapest, pp. 74-79. 

Mukerjee, A., and Ballard, D.H. 1985 (Mar. 25-28). Self-calibration in robot 
manipulators. Proc. IEEE Conf. Robotics and Automation. St. Louis, pp. 
1050-1057. 

Neuman, C.P., and Khosia, P.K. 1985 (May 29-31). Identification of robot 
dynamics: an application of recursive estimation. Proc. 4 th Yale Workshop on 
Applications of Adaptive Systems Theory. New Haven, pp. 42-49. 

Olsen, H.B., and Bekey, G.A. 1985 (Mar. 25-28). Identification of parameters in 
models of robots with rotary joints. Proc. IEEE Conf. Robotics and Automation. 
St. Louis, pp. 1045-1050. 

Samson, C. 1983 (Dec. 14-16). Robust nonlinear control of robotic manipulators. 
Proc. 22nd IEEE Conf. Decision and Control. San Antonio. 

Slotine, J.-J. E. 1985 The robust control of robot manipulators. Int. J. Robotics 


14 



Research. 4: (2): 49-64. 

Spong, M.W., Thorp, J.S., and Kleinwaks, J.M. 1984 (Dec. 12-14). The control 
of robot manipulators with bounded input. Part II: robustness and disturbance 
rejection. Proc. 2Srd IEEE Conf. Decision and Control. Las Vegas, pp. 

1047-1052. 

Symon, K.R. 1971 Mechanics. Reading, Mass.. Addison-Wesley 

Whitney, D.E., Lozinski, C.A., and Rourke, J.M. 1984 Industrial Robot 
Calibration Method and Results. CSDL-P-1879. Cambridge, Mass.. Charles Stark 
Draper Laboratory 


15 



APPENDIX 


Beginning with an n-joint manipulator, we assume each link i has a local coordinate 
system P,- with origin fixed at joint i; the vector p,- locates P,- with respect to a 
global coordinate system O (Figure 1). At the center of mass, a coordinate system 
Qt is aligned with the principle axes of inertia. The center of mass is located with 
respect to P,- by the vector c,- and with respect to the global origin O by q,. 

The inertial parameters of mass, center of mass, and moment of inertia are 
related to the motion of link i by the Newton-Euler equations: 

qfi = fa + m,g = rrii^i (H) 

Hit ^i ^ ^ii ^li^i “1“ X (g^i^t) 

where 

gfi = the net force acting on link t, 

^iii = the net torque about the center of mass of link t, 
fij = the force at joint i due to motion of link j alone, 

11,7 = the torque at joint i due to motion of link j alone, 
m, = the mass of link t, 

qli = the moment of inertia about the center of mass, 

Ui = the angular velocity vector, and 
g = the gravity vector (g = [0,0,-9.8 m/sec^]). 

Although the location of the center of mass and hence its acceleration q, are 
unknown, the latter is related to the acceleration p,- of joint i by (Symon, 1971): 

(13) 

(14) 

(15) 

Although the terms Ci x (w.- x Ci) and Ci x (wi x (a;,- x Ci)) are quadratic in the 
unknown location of the center of ma^s Ci, they may be eliminated by expressing 


Qi = Pt + Ui X Ci + Ui X (w,- X c,) 
Substituting into ( 11 ), which then substitutes into ( 12 ), 

fa = rrii (p,- - g) + w,- x m,c,- + w,- x (w,- x m,c,) 

= (g ~ P*) xm,c,- + -f mci X (d;,- x c,) 
4-a;,x(^I,w,) + rriiCi x (w,- x (a;,- x c,)) 
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the moment of inertia tensor about the joint i origin pl»- instead of about the center 
of mass gl,-. By the parallel axis theorem (Symon, 1971), 


pi.- = qli + m.-[(cfc.)l - (c.-cf)] 


(16) 


where 1 is the 3 dimensional identity matrix. Rewriting (15) and using (16), 

= (g - Pt) X miCi+qliOJi -H m,-[(cfc,)l - (c.-cf)]d;,- 

+Ui X (gl.-a;.)+w.- X (m.-[(cfc.)l - (c.-cf)]a;.) (17) 

= (g - Pt) X rriiCi+pliCJi + Ui X (pi,-a;.-) 

The following notation aids formulating a system of linear equations from those 
above: 


A r 1 

= [wxj c 


L J 



0 

-Uz 

<^y 


W X c = 

Uz 

0 

-<^x 



. -Wy 

w* 

0 



Iw = 


0 0 0 
0 Ux 0 Uy Uz 0 


0 0 (jJx 0 Uy Ug 



Ixx 


Ixx 

-1 

fxy 


Ixy 


Ixz 

= M 

Ixz 


lyy 

lyy 


lyz 


lyz 


- 1 

_ 1 


Izz 


where 


I = I^ = 


Ixz Ixy Ixz 

■^*1/ ^VV ^yz 
L ^xz lyz ^zz J 


A matrix equation can then be written from (14) and (17): 


fit 

n,i 


Pt-g [w.-x] + [a;,-x][w.-x] 


0 


0 


[(g-Wx] [•‘^tl + [w.-x][ •w.-jj 


m,- 

rriiCx. 

rriiCy, 

TYliCz- 

Ixx{ 




■^XZi 

^yvi 

^yz{ 


IZZf 


• J 
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